// -*- C++ -*-
/*!
 * @file  UsbCameraAcquire.cpp
 * @brief Sequence InPort component
 *
 */
#include "UsbCameraAcquire.h"

// Module specification
// <rtc-template block="module_spec">
static const char* col_spec[] =
  {
    "implementation_id", "UsbCameraAcquire",
    "type_name",         "UsbCameraAcquire",
    "description",       "UsbCameraAcquire component",
    "version",           "1.0",
    "vendor",            "Tomoaki Yoshikai, JSK",
    "category",          "example",
    "activity_type",     "DataFlowComponent",
    "max_instance",      "10",
    "language",          "C++",
    "lang_type",         "compile",
    "conf.default.print_name",         "0",
    ""
  };
// </rtc-template>

UsbCameraAcquire::UsbCameraAcquire(RTC::Manager* manager)
  : RTC::DataFlowComponentBase(manager),
    // <rtc-template block="initializer">
    m_ImageOut("imageout", m_ImageOutData)
    // </rtc-template>
{
}

UsbCameraAcquire::~UsbCameraAcquire()
{
}

RTC::ReturnCode_t UsbCameraAcquire::onInitialize()
{
  // Registration: InPort/OutPort/Service
  // <rtc-template block="registration">
  // Set InPort buffers
  addOutPort( "imageout", m_ImageOut );

  // Set service provider to Ports

  // Set CORBA Service Ports

  m_Width = 640;
  m_Height = 480;

  // </rtc-template>

  if (m_file.empty())
    bindParameter("file", m_file, "UsbCameraAcquireTest.conf");
  bindParameter("config", m_config, "UsbCameraAcquire0");
  bindParameter( "Color", m_Color, "0" );
  bindParameter( "WaitTime", m_WaitTime, "30" );
  
    return RTC::RTC_OK;
}

/*
RTC::ReturnCode_t UsbCameraAcquire::onFinalize()
{
  return RTC::RTC_OK;
}
*/

/*
RTC::ReturnCode_t UsbCameraAcquire::onStartup(RTC::UniqueId ec_id)
{
  return RTC::RTC_OK;
}
*/

/*
RTC::ReturnCode_t UsbCameraAcquire::onShutdown(RTC::UniqueId ec_id)
{
  return RTC::RTC_OK;
}
*/

RTC::ReturnCode_t UsbCameraAcquire::onActivated(RTC::UniqueId ec_id)
{
  m_cap = cvCreateCameraCapture(0);
  m_image = cvCreateImage( cvSize( m_Width, m_Height ), IPL_DEPTH_8U, 3 );
  cvSet( m_image, cvScalarAll( 0 ), NULL );
  cvSetCaptureProperty(m_cap, CV_CAP_PROP_FRAME_WIDTH, m_Width);
  cvSetCaptureProperty(m_cap, CV_CAP_PROP_FRAME_HEIGHT,  m_Height);

  fprintf(stderr, "Finish initialization\n");
  return RTC::RTC_OK;
}


RTC::ReturnCode_t UsbCameraAcquire::onDeactivated(RTC::UniqueId ec_id)
{
  cvReleaseCapture (&m_cap);
  return RTC::RTC_OK;
}


RTC::ReturnCode_t UsbCameraAcquire::onExecute(RTC::UniqueId ec_id)
{
  try
  {
    m_image = cvQueryFrame(m_cap);
    //アウトポートにデータを出力
    m_ImageOutData.pixels.length(m_Width*m_Height*3);
    m_ImageOutData.width  = m_Width;
    m_ImageOutData.height = m_Height;

    memcpy((void *)&(m_ImageOutData.pixels[0]), m_image->imageData,m_Width*m_Height*3);

    m_ImageOut.write();

  }
  catch(const std::runtime_error &e)
  {
      // 失敗しても続行
      std::cerr << e.what() << std::endl;
  }
  //

  return RTC::RTC_OK;
}

/*
RTC::ReturnCode_t UsbCameraAcquire::onAborting(RTC::UniqueId ec_id)
{
  return RTC::RTC_OK;
}
*/

/*
RTC::ReturnCode_t UsbCameraAcquire::onError(RTC::UniqueId ec_id)
{
  return RTC::RTC_OK;
}
*/

/*
RTC::ReturnCode_t UsbCameraAcquire::onReset(RTC::UniqueId ec_id)
{
  return RTC::RTC_OK;
}
*/

/*
RTC::ReturnCode_t UsbCameraAcquire::onStateUpdate(RTC::UniqueId ec_id)
{
  return RTC::RTC_OK;
}
*/

/*
RTC::ReturnCode_t UsbCameraAcquire::onRateChanged(RTC::UniqueId ec_id)
{
  return RTC::RTC_OK;
}
*/

extern "C"
{
 
  void UsbCameraAcquireInit(RTC::Manager* manager)
  {
    coil::Properties profile(col_spec);
    manager->registerFactory(profile,
                             RTC::Create<UsbCameraAcquire>,
                             RTC::Delete<UsbCameraAcquire>);
  }
  
};


